#include <bits/stdc++.h>
#include "main.h"
#include "init.h"
#include "astar.h"
#include <pthread.h>
using namespace std;

int in_map[N][N];
int color_flag = 1;    //颜色标号

int all_berth_path[n][n][berth_num]; // 地图上所有点到所有泊位的路径

/** 地图的联通块上色，不同的连通区域有不同的颜色判断(x1,y1)到(x2,y2)是否可到达，
    in_map[x1][y1]==in_map[x2][y2]即代表两点可到达可在机器人寻找泊位is_reach是否可到达
    和机器人寻找物品list<Goods>::iterator find_near_goods(Point &rbt_p)中判断
    **/
void init_connected()
{
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            // 该点是障碍物或者海洋，在映射地图上置为-1
            if (ch_map[i][j] == '*' || ch_map[i][j] == '#')
            {
                in_map[i][j] = -1;
                continue;
            }
            // 已经上了其他颜色，或者说是障碍物海洋，跳过
            if (in_map[i][j] != 0)
            {
                continue;
            }
            // 获取队列
            queue<std::pair<int, int>> q;
            // 初始化起始点
            q.push({i, j});
            in_map[i][j] = color_flag;
            // orientation 四个方向
            int dx[] = {-1, 0, 1, 0};
            int dy[] = {0, 1, 0, -1};
            while (!q.empty())
            {
                int x = q.front().first;
                int y = q.front().second;
                q.pop();
                // 遍历四个方向
                for (int i = 0; i < 4; ++i)
                {
                    int nx = x + dx[i];
                    int ny = y + dy[i];
                    if (nx >= 0 && ny >= 0 && nx < n && ny < n && in_map[nx][ny] == 0 && (ch_map[nx][ny] != '*' && ch_map[nx][ny] != '#'))
                    {
                        // 覆盖颜色，并加入队列中
                        in_map[nx][ny] = color_flag;
                        q.push({nx, ny});
                    }
                }
            }
            color_flag++;
        }
    }
}

// 初始化每个点去泊位的路径长度
void init_berth_path()
{
	for (int id = 0; id < berth_num; id++) {
		Berth &b = berth[id];
		queue<Point> q;
		int visit[n][n] = { 0 };
		q.push(b.p);
		visit[b.p.x][b.p.y] = 1;
		int dp = 1;
		int num = 1; // 每一层的点个数
		while (!q.empty()) {
			int temp = 0;
			for (int i = 0; i < num; i++) {
				Point &p = q.front();
				// 记录该点到泊位的路径长度
				if (ch_map[p.x][p.y] != '#' && ch_map[p.x][p.y] != '*') {
					all_berth_path[p.x][p.y][id] = dp;
				}
				
				// 遍历p周围的4个位置
				for (int j = 0; j < 4; j++) {
					int x_next = p.x + next_position[j][0];
					int y_next = p.y + next_position[j][1];
					if (x_next >= 0 && x_next < n && y_next >= 0 && y_next < n && ch_map[x_next][y_next] != '#' && ch_map[x_next][y_next] != '*' && !visit[x_next][y_next]) {
						q.push(Point(x_next, y_next));
						visit[x_next][y_next] = 1;
						++temp;
					}
				}
				q.pop();
			}
			num = temp;
			++dp; // 深度加一
		}
	}
}

// 线程任务
void *thread_task(void *arg) {
	// do some task
	
	pthread_exit(0);
}


